/**
 *
 */
package jp.seraph.jsade.model;

import jp.seraph.jsade.math.Angle;
import jp.seraph.jsade.math.Point3D;

import org.apache.commons.math.geometry.Rotation;
import org.apache.commons.math.geometry.Vector3D;

/**
 * ジョイントのデフォルト実装
 *
 */
public class DefaultJoint implements Joint {
    /**
     * 引数を指定しないコンストラクタです。 このコンストラクタを使用する場合、必ずinitメソッドを使用して初期化してください。
     */
    public DefaultJoint() {

    }

    public DefaultJoint(JointIdentifier aIdentifier, String aName, ModelPart aParent, ModelPart aChild, Vector3D aTranslation, Vector3D aAxis) {
        this.init(aIdentifier, aName, aParent, aChild, aTranslation, aAxis);
    }

    public void init(JointIdentifier aIdentifier, String aName, ModelPart aParent, ModelPart aChild, Vector3D aTranslation, Vector3D aAxis) {
        mIdentifier = aIdentifier;
        mName = aName;
        mParent = aParent;
        mChild = aChild;
        mTranslation = aTranslation;
        mAxis = aAxis;
        mMinAngle = mIdentifier.getMinAngle();
        mMaxAngle = mIdentifier.getMaxAngle();
        mAngle = Angle.ZERO;
    }

    private JointIdentifier mIdentifier;
    private String mName;
    private ModelPart mParent;
    private ModelPart mChild;
    private Vector3D mTranslation;
    private Vector3D mAxis;
    private Angle mMinAngle;
    private Angle mMaxAngle;

    /**
     * ジョイントの現在角度[°]
     */
    private Angle mAngle;

    // /**
    // * ジョイントの角速度[°/t]
    // */
    // private double mAngleVelocity;

    /**
     *
     * @see jp.seraph.jsade.model.Joint#getAnchor()
     */
    public Vector3D getTranslation() {
        return mTranslation;
    }

    /**
     *
     * @see jp.seraph.jsade.model.Joint#getAngle()
     */
    public Angle getAngle() {
        return mAngle;
    }

    /**
     * このジョイントの角度を設定する
     *
     * @param aAngle
     *            設定する角度[°]
     */
    public void setAngle(Angle aAngle) {
        mAngle = aAngle;
    }

    /**
     *
     * @see jp.seraph.jsade.model.Joint#getAxis()
     */
    public Vector3D getAxis() {
        return mAxis;
    }

    /**
     *
     * @see jp.seraph.jsade.model.Joint#getMaxAngle()
     */
    public Angle getMaxAngle() {
        return mMaxAngle;
    }

    /**
     *
     * @see jp.seraph.jsade.model.Joint#getMinAngle()
     */
    public Angle getMinAngle() {
        return mMinAngle;
    }

    /**
     *
     * @see jp.seraph.jsade.model.Joint#getIdentifier()
     */
    public JointIdentifier getIdentifier() {
        return mIdentifier;
    }

    /**
     *
     * @see jp.seraph.jsade.model.Joint#getName()
     */
    public String getName() {
        return mName;
    }

    /**
     *
     * @see jp.seraph.jsade.model.Joint#getObject()
     */
    public ModelPart getObject() {
        return mChild;
    }

    /**
     *
     * @see jp.seraph.jsade.model.Joint#getParentObject()
     */
    public ModelPart getParentObject() {
        return mParent;
    }

    /**
     *
     * @see jp.seraph.jsade.model.CoordinateSystem#getRelativeAngle()
     */
    public Angle getRelativeAngle() {
        return this.getAngle();
    }

    /**
     *
     * @see jp.seraph.jsade.model.CoordinateSystem#getRelativeAngleAsRad()
     */
    public double getRelativeAngleAsRad() {
        return this.getAngle().getRad();
    }

    /**
     *
     * @see jp.seraph.jsade.model.CoordinateSystem#isRoot()
     */
    public boolean isRoot() {
        return false;
    }

    /**
     *
     * @see jp.seraph.jsade.model.CoordinateSystem#getCoordinateAsRoot(jp.seraph.jsade.math.Point3D)
     */
    public Point3D getCoordinateAsRoot(Point3D aLocalPoint) {
        // この座標系からaLocalPointへのベクトルを作る
        Vector3D tLotatedVector = new Vector3D(aLocalPoint.getX(), aLocalPoint.getY(), aLocalPoint.getZ());

        // Angleで回転させる
        Rotation tRotation = new Rotation(this.getAxis(), this.getRelativeAngleAsRad());
        tLotatedVector = tRotation.applyTo(tLotatedVector);

        // 親オブジェクトから見たaLocalPoint座標は
        // 親オブジェクトからこのジョイントへの平行移動に回転させたベクトルを足したもの
        Point3D tRerativePoint = Point3D.origin.add(this.getTranslation()).add(tLotatedVector);

        return this.getParentObject().getCoordinateAsRoot(tRerativePoint);

    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelObject#getAbsoluteCoordinate()
     */
    public Point3D getAbsoluteCoordinate() {
        return getCoordinateAsRoot(Point3D.origin);
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelObject#getCentorOfGravity()
     */
    public Point3D getCentorOfGravity() {
        return this.getAbsoluteCoordinate();
    }

    /**
     * ジョイントは便宜上、質量0のオブジェクトとして扱う
     *
     * @see jp.seraph.jsade.model.ModelObject#getMass()
     */
    public double getMass() {
        return 0;
    }

}
